/*
navigation_mag.c
offer magnetism navigation function
designed by lunanting

-------------------------
2020-03-26 17:41:31
create file
-------------------------
*/

#include "navigation_mag.h"
#include "cli_cmd.h"
#include "string.h"

static MAG_state mag_state={0};
const uint8_t location_head[]={0x52,0x4d,0x70,0x6F,0x73};
const uint8_t road_choose_head[]={0x47,0x55,0x69,0x73,0x65};


/*
mag init
param:
    port:uart port
    agre:agreement
    speed:uart speed,当agreement=modbus协议时，设置无效
    slava_id:slave device id,当agreement=hs私有协议时，设置无效
return:init state
*/
int xp_mag_init(uint8_t port,MAG_agreement agreement,int speed,uint8_t slave_id){
    uart_dev_t *uart=&mag_state.uart;
    int ret=0;

    if(mag_state.init==1)return 0;
    mag_state.agreement=agreement;
    if(agreement==mag_hs){  //hs私有协议
        uart->port=port;
        uart->config.baud_rate=speed;
        uart->config.data_width=DATA_WIDTH_8BIT;
        uart->config.parity=NO_PARITY;
        uart->config.stop_bits=STOP_BITS_1;
        uart->config.flow_control=FLOW_CONTROL_DISABLED;
        uart->config.mode=MODE_TX_RX;

        ret=hal_uart_init(uart);
        if(ret!=0){
            println("mag hs init fail! hal_uart_init() error code=%d",ret);
            ret=-1;
        }
        else{
            println("mag module init success! as 485 HS agreement.");
        }
        mag_state.init=2;
    }
    else if(agreement==mag_modbus){ //modbus协议
        mag_state.modbus_port=port;
        mag_state.modbus.resBuf=mag_state.read_data;
        mag_state.modbus.resLen=&mag_state.read_len;
        if(slave_id<2||slave_id>127){
            mag_state.modbus.slave_id=MAG_SLAVE_ID;
        }
        else{
            mag_state.modbus.slave_id=slave_id;
        }

        ret=xp_modbus_init(port,speed,MB_PAR_NONE,1000);
        if(ret!=0){
            println("mag modbus init fail! xp_modbus_init() code=%d",ret);
            ret=-2;
        }
        else{
            println("mag module init success ! as modbus agreement.");
        }
        ret=xp_mag_set_dir(mag_middle);
        if(ret!=0){
            println("mag module set dir fail! set dir fail code=%d",ret);
            ret=-4;
        }
        else{
            println("mag moule set dir success!dir =middle.");
        }
        mag_state.init=1;
    }
    else{
        mag_state.init=0;
        ret=-3;
    }
    return ret;
}



/*
get mag location
param:
    data:save data
return:
    0=success
    -1=get location data fail
    -10:save location buffer=NULL
    -11:mag module not init
*/
int xp_mag_location(MAG_location *data){
    int ret=0;

    if(data==NULL)return -10;

    if(mag_state.init==1){
        ret=xp_mag_get_location_modbus();
    }
    else if(mag_state.init==2){
        ret=xp_mag_get_location_hs();
    }
    else{
        return -11;
    }

    if(ret==0){
        memcpy(data,&mag_state.location,sizeof(MAG_location));
        return 0;
    }
    else{
        return -1;
    }
}



/*
analysis mag recive data
param:
    data:recive data
return:
    0=success
    -1:input data buffer=MULL
    -2:unknow data type
*/
int xp_mag_recive_analysis_hs(uint8_t *data){
    // int ret=0;

    if(data==NULL)return -1;

    if(memcmp(data,location_head,5)==0){
        mag_state.location.sensor=data[6];
        mag_state.location.mag=data[7];
        mag_state.location.left=(data[8]<<8)+data[9];
        mag_state.location.middle=(data[10]<<8)+data[11];
        mag_state.location.right=(data[12]<<8)+data[13];
    }
    else if(memcmp(data,road_choose_head,5)==0){

    }
    else{
        return -2;
    }
    return 0;
}

/*
get mag location by HS agreement
return：
    0=success
    -1=uart send cmd error
    -2=uart recive data error
    -3=crc error
    -4=analysis error
	-5=no recive or too short
*/
int xp_mag_get_location_hs(void){
    int ret=0;
    uint16_t crc=0,c;

    uint8_t cmd[10]={0x52,0x4d,0x70,0x6f,0x73};
    uint8_t rec_buf[20];
    uint32_t rec_len;

    crc=xp_mag_crc(cmd,5);
    cmd[5]=crc;
    cmd[6]=crc>>8;

    ret=hal_uart_send(&mag_state.uart,cmd,7,10);
    if(ret!=0){
        println("error:xp_mag_laocation() uart send cmd fail! code=%d",ret);
        return -1;
    }
    ret=hal_uart_recv_II(&mag_state.uart,rec_buf,20,&rec_len,100);
    if(ret!=0){
        println("error:xp_mag_laocation() uart rec data fail! code=%d",ret);
        return -2;
    }
    crc=xp_mag_crc(rec_buf,rec_len-2);
	if(rec_len<3)return -5;
    c=rec_buf[rec_len-2]+(rec_buf[rec_len-1]<<8);
    if(crc!=c){
        return -3;
    }

    ret=xp_mag_recive_analysis_hs(rec_buf);
    if(ret!=0)return -4;
    return 0;
}



/*
mag recive data from modbus
return:
    0=success

*/
int xp_mag_recive_analysis_modbus(void){
    int ret=0;

    if(mag_state.read_len!=4){
        return -1;
    }
    mag_state.location.mag=(mag_state.read_data[0]<<8)+mag_state.read_data[1];
    mag_state.location.middle=(mag_state.read_data[2]<<8)+mag_state.read_data[3];
    return 0;
}




/*
get mag location by modbus agreement
return:
    0=success
    -1=get location fail
    -2=recive data analysis fail
*/
int xp_mag_get_location_modbus(void){
    int sta=0;

    mag_state.modbus.mb_fun=mb_common_r;
    mag_state.modbus.fun_code=0x04;
    mag_state.modbus.start_addr=MAG_STATE_RES;
    mag_state.modbus.rwLen=2;
    
    sta=xp_modbus_ask(mag_state.modbus_port,&mag_state.modbus);
    if(sta!=0){
        return -1;
    }
    sta=xp_mag_recive_analysis_modbus();
    if(sta!=0){
        return -2;
    }
    return 0;
}



/*
set mag road choose
param:
    dir:set mag dir choose
return:
    0=success
    -1:set fail
*/
int xp_mag_set_dir(MAG_dir dir){
    int sta=0;
    uint16_t dir_type=(uint16_t)dir;

    mag_state.modbus.mb_fun=mb_common_w;
    mag_state.modbus.fun_code=0x10;
    mag_state.modbus.start_addr=MAG_DIR_RES;
    mag_state.modbus.rwLen=1;
    mag_state.modbus.wBuf=&dir_type;
    sta=xp_modbus_ask(mag_state.modbus_port,&mag_state.modbus);
    if(sta!=0){
        return -1;
    }
    return 0;
}

/*
mag  crc
param:
    data:crc data
    len:data length
return:crc value
*/
uint16_t xp_mag_crc(uint8_t *data,int len){
    int i,j;
    uint16_t wcrc=0xffff;
    uint16_t wp=0xa001;

    for(i=0;i<len;i++){
        wcrc^=data[i];
        for(j=0;j<8;j++){
            if(wcrc&0x0001){
                wcrc=(wcrc>>1)^wp;
            }
            else{
                wcrc=wcrc>>1;
            }
        }
    }
    return wcrc;
}

/******************************test*********************/
void xp_mag_test(void){
    MAG_location data;
    int rec=0;
    static int count=0;

    rec=xp_mag_location(&data);
    if(rec==0){
        count++;
        println("%d mag location:sensor=%d,mag=%d	middle=%d",
                                count,data.sensor,data.mag,data.middle);
    }
    else{
        println("location data get fail! code=%d",rec);
    }
}


/*
mag debug
*/
int xp_mag_debug(char *type,char *fun,char *param){
    if(strcmp(type,"mag")!=0)return 0;
    if(strcmp(fun,"read")==0){
        xp_mag_test();
    }

    else{
        return 0;
    }
    return 1;
}

//end of the file

